/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Estimate the rotation needed to minimize the RMSE
% between GPS and laser odometry trajectories
*  
*/

#include "EstimateRotation.h"


void MatchPairPoints(TFPoint RefPoints[], TFPoint ActualPoints[], int N,
    float *Tx, float *Ty, float *fi){

    float x=0,y=0,xr=0,yr=0;
    float Sum_xx=0,Sum_yy=0,Sum_xy=0,Sum_yx=0;
    int i;

    if (N<3) {
      fprintf(stderr,"MatchPairPoints::Low number of points\n");
      *Tx=0;
      *Ty=0;
      *fi=0;
      return;
    }

    for(i=0; i<N; i++) {
      xr += RefPoints[i].X;
      yr += RefPoints[i].Y;
      x  += ActualPoints[i].X;
      y  += ActualPoints[i].Y;
    }

    x = x/(float)N;   y = y/(float)N;
    xr= xr/(float)N;  yr= yr/(float)N;

    for(i=0; i<N; i++) {
      Sum_xx += (RefPoints[i].X - xr) * (ActualPoints[i].X - x);
      Sum_yy += (RefPoints[i].Y - yr) * (ActualPoints[i].Y - y);
      Sum_xy += (RefPoints[i].X - xr) * (ActualPoints[i].Y - y);
      Sum_yx += (RefPoints[i].Y - yr) * (ActualPoints[i].X - x);
    }

    *fi = atan((Sum_xy-Sum_yx)/(Sum_xx+Sum_yy));
    *Tx = x - (xr*cos(*fi)-yr*sin(*fi));
    *Ty = y - (xr*sin(*fi)+yr*cos(*fi));
}


////////////////////////////////////////////////////////////////////////////////////////////////////
/// READ TWO POSE FILES
////////////////////////////////////////////////////////////////////////////////////////////////////
bool readTwoPoseFiles(const char *positionFile1, const char *positionFile2, int &N, int &counter2){
	 CPositionFileReader preader1;
	 CPositionFileReader preader2;
	 
 
	 preader1.ReadFile(positionFile1);
	 preader2.ReadFile(positionFile2);

	 
	///////////////////////////////////////////////////////////////////////
	/// MAKE SYNCHRONIZATION
	///////////////////////////////////////////////////////////////////////
	std::list<gim::time> times;
  
	if(preader2.GetTimes(times)){
    dPrint(1,"Got times... number of times %d",times.size());
  }else{
    dPrint(1,"Failed to get times ");
    exit(1);
  }
	
	MaCI::Position::CPositionData pdata1;
	MaCI::Position::CPositionData pdata2;

	std::list<gim::time>::iterator ii;
	N = times.size();

	poses1 = new TFPoint[N];
	poses2 = new TFPoint[N];
	poses3 = new TFPoint[N];

	int counter1 = 0;
	counter2 = 0;
	float dist = 0.0;

	for(ii=times.begin();ii!=times.end();ii++){
		  if(preader2.GetPositionData(pdata2,*ii)){
				if(preader1.getClosestTo(pdata1,*ii)){

					const MaCI::Position::TPose2D *pose1; /// pose 1
					const MaCI::Position::TPose2D *pose2; /// pose 2

					
					pose1 = pdata1.GetPose2D();
					pose2 = pdata2.GetPose2D();

					if(pose1 == NULL || pose2 == NULL){
						fprintf(stderr,"FAILED TO PARSE POSITION!!\n");
						exit(1);
					}

						TFPoint p2; 
						p2.X = pose2->x;
						p2.Y = pose2->y;
						p2.angle = pose2->a;
						p2.s = (uint)ii->getSeconds();
						p2.us = (uint)ii->getUSeconds();
						poses3[counter2] = p2;											
						counter2 += 1;					


					if(counter1 > 0){
							dist = sqrt(pow(poses1[counter1-1].X-pose1->x,2) + pow((poses1[counter1-1].Y-pose1->y),2));					
						if(dist>0.1){
								TFPoint p1; 
								p1.X = pose1->x;
								p1.Y = pose1->y;
								poses1[counter1] = p1;

								TFPoint p2; 
								p2.X = pose2->x;
								p2.Y = pose2->y;
								poses2[counter1] = p2;
								counter1 += 1;															
								}
					}else{
						TFPoint p1; 
						p1.X = pose1->x;
						p1.Y = pose1->y;
						poses1[counter1] = p1;

						TFPoint p2; 
						p2.X = pose2->x;
						p2.Y = pose2->y;
						poses2[counter1] = p2;											
						counter1 += 1;

					}
				}else{
					dPrint(1,"Failed to get position2");
					exit(1);
				}	
			}else{
				dPrint(1,"Failed to get position1");
			return false;
		}
	}

	return true;
}

bool 	RotatePoints(TFPoint poses[], float Tx, float Ty, float fi, int N){
	
		
	  poses4 = new TFPoint[N];
		
		float R[2][2] = {{cos(fi), sin(fi)},  
											{-sin(fi), cos(fi)}};
    
		for(int i=0; i<N; i++) {				
			 TFPoint p4;
			 float MovedPoseX = poses[i].X+Tx-poses[0].X; 
			 float MovedPoseY = poses[i].Y+Ty-poses[0].Y;
			 p4.X = (R[0][0]*MovedPoseX + R[0][1]*MovedPoseY) + poses[0].X;
			 p4.Y = (R[1][0]*MovedPoseX + R[1][1]*MovedPoseY) + poses[0].Y;
 			 p4.angle = poses[i].angle - fi;
			 p4.s = poses[i].s;
			 p4.us = poses[i].us;

		 	 poses4[i] = p4;
  	}
	return true;
}

bool WritePointsToFile(TFPoint poses[],int N){

  FILE * pFile;
	pFile = fopen("RotatedPoints.txt","w");	
	fprintf(pFile,"%%MaCI_Position log file 2.0\n");
	fprintf(pFile,"%%Group: Log Instance: Matched + Rotated\n");
	fprintf(pFile,"%%timestamp_sec timestamp_usec position_x position_y position_z angle_x angle_y angle_z variance_x variance_y variance_z variance_angleX variance_angleY variance_angleZ probability speed angularSpeed\n\n");

	for(int i = 0; i<N; i++){
		fprintf(pFile,"%d %d %.6f %.6f 0.000000 0.000000 0.000000 %.6f 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000\n", 									 poses[i].s, poses[i].us, poses[i].X, poses[i].Y, poses[i].angle);
	}
	fclose(pFile);
	return true;
}

/***********************
******** MAIN **********
************************/
int main(int argc, char *argv[]){
	
	if(argc !=3){
		fprintf(stderr,"\nUsage: ./EstimateRotation [GPStrajectory.file] [LaserTrajectory.file]\n");
		exit(1);
	}

	int Ngps = 0;
	int Nm = 0;
	float Tx, Ty, fi;

	char *GPSFile = argv[1];	
	char *laserFile = argv[2];

	readTwoPoseFiles(GPSFile, laserFile, Ngps, Nm);	
	MatchPairPoints(poses1, poses2, Ngps, &Tx, &Ty, &fi);
	RotatePoints(poses3, Tx, Ty, fi, Nm);
	WritePointsToFile(poses4,Nm);

	fprintf(stderr,"Tx: %.4f, Ty: %.4f, fi: %.4f\n",Tx,Ty,fi);	

	return 0;
}
